﻿#include "Tutorial.h"
using namespace std;
using namespace ASL;

// 线性代数运算案例
void Tutorial::LinearAlgebraTutorial(void)
{
    cout << "------ Linear Algebra Tutorial ------" << endl
         << endl;

    // 通过构造函数创建指定维度矩阵
    Matrix A(5, 5), B, H;

    // 通过函数运算符访问指定行列元素
    A(0, 0) = 0.8147;
    A(0, 1) = 0.0975;
    A(0, 2) = 0.1576;
    A(0, 3) = 0.1419;
    A(0, 4) = 0.6557;
    A(1, 0) = 0.9058;
    A(1, 1) = 0.2785;
    A(1, 2) = 0.9706;
    A(1, 3) = 0.4218;
    A(1, 4) = 0.0357;
    A(2, 0) = 0.1270;
    A(2, 1) = 0.5469;
    A(2, 2) = 0.9572;
    A(2, 3) = 0.9157;
    A(2, 4) = 0.8491;
    A(3, 0) = 0.9134;
    A(3, 1) = 0.9575;
    A(3, 2) = 0.4854;
    A(3, 3) = 0.7922;
    A(3, 4) = 0.9340;
    A(4, 0) = 0.6324;
    A(4, 1) = 0.9649;
    A(4, 2) = 0.8003;
    A(4, 3) = 0.9595;
    A(4, 4) = 0.6787;

    cout << "Matrix A" << endl;
    cout << A << endl;

    // 矩阵基本运算
    cout << "Matrix (A+2*A^T)*A*|A|" << endl;
    H = (A + Matrix::transpose(A) * 2.0) * A * Matrix::determinant(A);
    cout << H << endl;

    // 矩阵求逆运算
    cout << "Matrix A^-1" << endl;
    H = Matrix::inverse(A);
    cout << Matrix::inverse(A) << endl;

    // 相似变换矩阵
    cout << "Matrix Householder" << endl;
    H = Matrix::householder(A.columnVector(0), 0);
    cout << H << endl;

    cout << "Matrix Gauss" << endl;
    H = Matrix::gauss(A.columnVector(0), 0);
    cout << H << endl;

    cout << "Matrix Givens" << endl;
    H = Matrix::givens(A.columnVector(0), 0, 1);
    cout << H << endl;

    // 特殊构型矩阵
    H = Matrix::schmidt(A);
    cout << "Matrix Schmidt" << endl;
    cout << H << endl;

    H = Matrix::hessenberg(A);
    cout << "Matrix Hessenberg" << endl;
    cout << H << endl;

    H = Matrix::schur(A);
    cout << "Matrix Schur" << endl;
    cout << H << endl;

    // 矩阵正交分解QR/三角分解LU
    Matrix Q, R, L, U;

    Matrix::householderQR(A, Q, R);

    cout << "Matrix Q" << endl;
    cout << Q << endl;
    cout << "Matrix R" << endl;
    cout << R << endl;

    Matrix::gaussLU(A, L, U);

    cout << "Matrix L" << endl;
    cout << L << endl;
    cout << "Matrix U" << endl;
    cout << U << endl;

    // 矩阵特征值求解
    vector<complex<double>> eigen;

    eigen = Matrix::eigenValue(A);

    cout << "Matrix Eigen" << endl;

    for (int n = 0; n < eigen.size(); n++)
    {
        Basic::display(cout, eigen[n]);
        cout << endl;
    }

    cout << endl;
}

// 线性插值计算案例
void Tutorial::LinearInterpolationTutorial(void)
{
    cout << "------ Linear Interpolation Tutorial ------" << endl
         << endl;

    // 通过数组定义插值数据序列
    // 插值数轴X alpha = [0, 2]
    // 插值数轴X beta = [-2, -1, 0, 1, 2]
    // 插值数轴Y gamma = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
    // 函数关系 gamma = f(alpha, beta)
    double alpha[2] = {0, 2};
    double beta[5] = {-2, -1, 0, 1, 2};
    double gamma[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    double value;

    // 使用静态公有函数直接进行插值计算
    value = LookupTable::interp(2, 5, alpha, beta, gamma, 2, 0, false);

    // 创建数据插值表对象
    LookupTable A, B;

    // 使用预设数组设置线性插值表对象数据内容
    A.setX(2, alpha);
    A.setX(5, beta);
    A.setY(10, gamma);

    // 检查线性插值表数据合规性
    bool ok;

    ok = A.isVaild();
    cout << "LookupTable A" << endl;
    cout << A << endl;

    // 将线性插值表对象存储为标准格式的本地数据文件
    A.save("LookupData.txt");

    // 读取外部数据文件以初始化线性插值表对象
    ok = B.load("LookupData.txt");
    cout << "LookupTable B" << endl;
    cout << B << endl;

    // 使用线性插值表的公有函数进行线性插值计算
    value = B.interp(0, 1, false);
    cout << endl;
}

// 数值积分求解案例
void Tutorial::CalculusTutorial(void)
{
    cout << "------ Calculus Tutorial ------" << endl
         << endl;

    // 创建积分器对象
    Calculus A;

    // 初始化积分器配置
    // 计算模式 数值积分
    // 积分初值 0.0
    // 计算步长 0.005s
    // 积分方法 欧拉法
    A.initialize(Calculus::ModeType::Integrate, 0.0, 0.005, Calculus::MethodType::Euler);

    // 设置积分限幅
    // 是否启用限幅功能 true/false
    // 计算限幅值域 -0.5~+0.5
    A.limit(false);
    A.limit(-0.5, 0.5);

    // 模拟计算时序循环 0:0.005:0.5s
    for (int n = 0; n < 100; n++)
    {
        double t, u, y;

        // 当前仿真时刻
        t = n * 0.005;

        // 当前计算周期输入信号
        u = sin(t * 2.0 * PI);

        // 执行单步计算流程并得到当前周期计算结果
        y = A.step(u);

        // 访问前1个计算周期的0阶数值
        y = A.value(Calculus::ValueType::X, 1);

        // 访问前1个计算周期的1阶数值
        y = A.value(Calculus::ValueType::XDot, 1);
    }
}

// 线性系统分析案例
void Tutorial::LinearSystemTutorial(void)
{
    cout << "------ Linear System Tutorial ------" << endl
         << endl;

    // 案例系统结构框图
    // u -->O--> PID --> G --.--> y
    //     -^                |
    //      |----------------|

    // 创建被控对象传递函数 G
    double T1 = 0.11, T2 = 3.0, E = 0.5, V = 306, K1 = 0.0116, K2 = 1.54;
    double Kp = 1.0, Ki = 0.25, Kd = 1.0;
    Polynomial num, den;
    TransferFunction A, B, C, G, PID;

    num = Polynomial{1};
    den = Polynomial{1, 2 * T1 * E, T1 * T1};
    A = TransferFunction(num, den);

    num = Polynomial{1};
    den = Polynomial{1, T2};
    B = TransferFunction(num, den);

    num = Polynomial{1};
    den = Polynomial{0, 1};
    C = TransferFunction(num, den);

    G = A * B * C * K1 * V;

    // 创建控制器传递函数 PID
    num = Polynomial{1};
    den = Polynomial{0, 1};
    B = TransferFunction(num, den);

    num = Polynomial{0, 1};
    den = Polynomial{1};
    C = TransferFunction(num, den);

    PID = C * Kd + B * Ki + Kp;

    // 计算系统开环传递函数
    TransferFunction G_OPEN;

    G_OPEN = PID * G;

    cout << "TransferFunction Open-Loop" << endl
         << endl;
    cout << G_OPEN << endl;

    // 计算开环传递函数的零极点
    TransferFunction::ZeroPoleInfo zpk_info;

    zpk_info = TransferFunction::zeroPole(G_OPEN);

    cout << "TransferFunction Zero" << endl;

    for (int n = 0; n < zpk_info.zero.size(); n++)
    {
        Basic::display(cout, zpk_info.zero[n]);
        cout << endl;
    }

    cout << endl;

    cout << "TransferFunction Pole" << endl;

    for (int n = 0; n < zpk_info.pole.size(); n++)
    {
        Basic::display(cout, zpk_info.pole[n]);
        cout << endl;
    }

    cout << endl;

    cout << "TransferFunction K" << endl;
    cout << zpk_info.k << endl;
    cout << endl;

    // 计算开环传递函数的对数频率特性
    vector<double> omega, phase, gain;

    // 自定义频率序列 rad/s
    // 计算频率序列 [0.01, 0.1, 1, 10, 100] rad/s
    omega = vector<double>{0.01, 0.1, 1, 10, 100};
    TransferFunction::bode(G_OPEN, omega, gain, phase);

    // 默认计算 0.01~100 rad/s 区间的对数频率特性
    omega.clear();
    TransferFunction::bode(G_OPEN, omega, gain, phase);

    cout << "TransferFunction Bode" << endl;

    // 记录本地数据文件
    ofstream stream;

    stream.open("BodeData.txt");

    if (stream.is_open())
    {
        stream << "Omega(rad/s)\tGain(dB)\tPhase(deg)" << endl;
        cout << "Omega(rad/s)\tGain(dB)\tPhase(deg)" << endl;

        for (int n = 0; n < omega.size(); n++)
        {
            stream << omega[n] << "\t" << gain[n] << "\t" << phase[n] << endl;

            if (n % 900 == 0)
            {
                cout << omega[n] << "\t" << gain[n] << "\t" << phase[n] << endl;
            }
        }

        stream.close();
    }

    cout << endl;

    // 计算开环传递函数的幅值/相位裕度
    TransferFunction::MarginInfo margin_info;

    // 计算幅值/相位裕度
    margin_info = TransferFunction::margin(omega, gain, phase);

    cout << "TransferFunction Margin" << endl;
    cout << "Gm(dB)\t" << margin_info.Gm << endl;
    cout << "Pm(deg)\t" << margin_info.Pm << endl;
    cout << "Wcg(rad/s)\t" << margin_info.Wcg << endl;
    cout << "Wcp(rad/s)\t" << margin_info.Wcp << endl;
    cout << endl;

    // 计算系统闭环传递函数
    TransferFunction G_CLOSE;

    // 开环传递函数作为前向通路的单位负反馈回路
    num = Polynomial{1};
    den = Polynomial{1};
    C = TransferFunction(num, den);

    G_CLOSE = TransferFunction::feedback(PID * G, C, -1.0);
    cout << "TransferFunction Closed-Loop" << endl
         << endl;
    cout << G_CLOSE << endl;

    // 计算闭环传递函数的离散化形式
    TransferFunction G_DISC;

    // 采样时间 0.005s
    G_DISC = TransferFunction::discrete(G_CLOSE, 0.005);
    cout << "TransferFunction Closed-Loop Discrete" << endl
         << endl;
    cout << G_DISC << endl;

    // 计算闭环传递函数的阶跃响应
    vector<double> time, value;

    // 允许自定义计算周期与终止时间
    // 计算时域 0:0.01:5s
    TransferFunction::step(G_CLOSE, time, value, 0.01, 5.0);

    // 默认计算 0:0.005:10s 区间的阶跃响应
    TransferFunction::step(G_CLOSE, time, value);

    cout << "TransferFunction Step" << endl;

    // 记录本地数据文件
    stream.open("StepData.txt");

    if (stream.is_open())
    {
        stream << "Time(s)\tValue(-)" << endl;

        for (int n = 0; n < time.size(); n++)
        {
            stream << time[n] << "\t" << value[n] << endl;

            if (n % 200 == 0)
            {
                cout << time[n] << "\t" << value[n] << endl;
            }
        }

        stream.close();
    }

    cout << endl;

    // 计算系统的时域性能指标
    TransferFunction::StepInfo step_info;

    step_info = TransferFunction::step(time, value);

    cout << "TransferFunction StepInfo" << endl;
    cout << "RiseTime(s)\t" << step_info.riseTime << endl;
    cout << "PeakTime(s)\t" << step_info.peakTime << endl;
    cout << "SettingTime(s)\t" << step_info.settingTime << endl;
    cout << "PeakValue(-)\t" << step_info.peakValue << endl;
    cout << "FinalValue(-)\t" << step_info.finalValue << endl;
    cout << "OverShoot(-)\t" << step_info.overShoot << endl;
    cout << endl;
}

// 坐标系算法案例
void Tutorial::CoordinateTutorial(void)
{
    cout << "------ Coordinate Tutorial ------" << endl
         << endl;

    // 计算坐标系旋转矩阵
    Matrix A;

    // 绕X轴旋转 30 deg
    A = Coordinate::rotationX(30.0);

    // 计算坐标系投影矩阵
    // 旋转关系 导航系 > 机体系
    // 滚动角 0.0 deg  航向角 0.0 deg 俯仰角 30.0 deg
    A = Coordinate::cNav2Body(0.0, 0.0, 30.0);

    // 计算欧拉角 [gamma, psi, zeta] deg
    // 坐标轴旋转顺序 Y>Z>X
    Vector eular;

    eular = Coordinate::eular(A, Coordinate::RotationType::YZX);

    // 计算四元数 [q0, q1, q2, q3]
    Vector Q;

    Q = Coordinate::quaternion(A);

    // 计算攻角 deg
    double alpha;
    Vector V(3);

    // 设置机体系速度向量数值 [1.0, 0.0, 0.0] m/s
    V(AXIS_X) = 1.0;
    V(AXIS_Y) = 0.0;
    V(AXIS_Z) = 0.0;

    // 当速度小于 0.01 m/s 时默认攻角为零
    alpha = Coordinate::alpha(V, 0.01);
}

// 地球几何算法案例
void Tutorial::GeodesyTutorial(void)
{
    cout << "------ Geodesy Tutorial ------" << endl
         << endl;

    // 计算指定纬度 deg 位置的主曲率半径 m
    double Rm, Rn;

    Rm = Geodesy::radiusM(39.0);
    Rn = Geodesy::radiusN(39.0);

    // 计算指定纬度 deg 和 海拔高度 m 位置的重力加速度 m/s^2
    double g;

    g = Geodesy::gravity(39.0, 0.0);

    // 计算 起点经纬度 至 终点经纬度 的 大地距离 m 和 方位角 deg
    double S, A;
    double longitudeA = 122.0, latitudeA = 39.0;
    double longitudeB = 122.0, latitudeB = 39.0;

    Geodesy::vincentSA(longitudeA, latitudeA, longitudeB, latitudeB, S, A);

    // 换算 地心地固系坐标 ECEF 与 经纬高 LLA
    Vector ECEF(3), LLA(3);

    LLA(AXIS_LONGITUDE) = 122.0;
    LLA(AXIS_LATITUDE) = 39.0;
    LLA(AXIS_ALTITUDE) = 0.0;

    ECEF = Geodesy::cLLA2ECEF(LLA(AXIS_LONGITUDE), LLA(AXIS_LATITUDE), LLA(AXIS_ALTITUDE));
    LLA = Geodesy::cECEF2LLA(ECEF(AXIS_X), ECEF(AXIS_Y), ECEF(AXIS_Z));
}

// 自定义模型设计与仿真案例
void Tutorial::CustomSimulationTutorial(void)
{
    cout << "------ Custom Simulation Tutorial ------" << endl
         << endl;

    // 案例系统结构框图
    // ClockModel ---> time
    // SineModel ----> sin
    //            |--> IntegrateModel --> i(sin)
    //            |--> DerivateModel ---> d(sin)

    // 仿真引擎组件
    ASLSimulator simulator;

    // 仿真模型系列
    ClockModel model_clock;
    SineModel model_sine;
    IntegrateModel model_i;
    DerivateModel model_d;

    // 模型参数配置：时钟模型
    model_clock.setup();

    // 模型参数配置：正弦信号模型
    model_sine.vAmplitude->setReal(1.0);
    model_sine.vFrequency->setReal(1.0);
    model_sine.vOffset->setReal(0.0);
    model_sine.setup();

    // 模型参数配置：积分运算模型
    model_i.vInitialValue->setReal(0.0);
    model_i.setup();

    // 模型参数配置：微分运算模型
    model_d.vInitialValue->setReal(0.0);
    model_d.setup();

    // 设置参与仿真的模型对象
    // 仿真计算时序：0 时钟模型 > 1 正弦信号模型 > 2 积分运算模型 > 3 微分运算模型
    simulator.setModel(&model_clock);
    simulator.setModel(&model_sine);
    simulator.setModel(&model_i);
    simulator.setModel(&model_d);

    // 配置模型对象之间的输入/输出关系
    // 正弦信号模型.输出信号y > 积分运算模型.输入信号u
    // 正弦信号模型.输出信号y > 微分运算模型.输入信号u
    simulator.setLink(model_sine.vY, model_i.vU);
    simulator.setLink(model_sine.vY, model_d.vU);

    // 本地仿真结果数据文件
    ofstream stream;

    stream.open("CustomSimulationData.txt");

    // 设置待记录的模型端口
    simulator.setRecordData(model_clock.vT);
    simulator.setRecordData(model_sine.vY);
    simulator.setRecordData(model_i.vY);
    simulator.setRecordData(model_d.vY);

    // 仿真引擎流程方法（引擎配置）
    // 仿真时间：0~1s 仿真步长：0.005s 求解器类型：欧拉法ODE-1
    simulator.setup(0.0, 1.0, 0.005, ASLSimulator::SolverType::Euler);

    // 仿真引擎流程方法（初始化）
    simulator.initialize();
    simulator.save(stream, true);

    // 运行仿真试验流程
    // 当仿真引擎运行状态不为单步执行状态时退出
    while (simulator.phase() == ASLSimulator::PhaseType::Step)
    {
        // 仿真引擎流程方法（单步运行）
        simulator.step();
        simulator.save(stream, false);
    }

    // 仿真引擎流程方法（仿真终止）
    simulator.terminate();
    stream.close();
}

// 典型飞行器运动学仿真案例
void Tutorial::AircraftSimulationTutorial(void)
{
    cout << "------ Aircraft Simulation Tutorial ------" << endl
         << endl;

    // 自定义仿真工程
    AircraftSimulation simulation;

    // 仿真准备流程
    simulation.setup();

    // 仿真初始化流程
    // 数据文件路径请按实际情况修改 例如 "D:/ASL/data"
    simulation.initialize("../../data");

    // 仿真单步运行流程
    simulation.step();
}
